#include <Servo.h>
#include <ArduinoJson.h>
#include <ESP8266WiFi.h>

WiFiServer server(23);

enum {
  S_M,
  S_L,
  S_R,
  S_C,
};

Servo servo[4];

void setup() {
  //Servo库的默认脉冲时间为544-2400μs，需要的脉冲为500-2500μs
  servo[S_M].attach(D5, 500, 2500);
  servo[S_L].attach(D6, 500, 2500);
  servo[S_R].attach(D7, 500, 2500);
  servo[S_C].attach(D8, 500, 2500);

  //初始位置
  servo[S_M].write(90);
  servo[S_L].write(90);
  servo[S_R].write(90);
  servo[S_C].write(60);
  
  Serial.begin(74880);
  WiFi.mode(WIFI_STA);
  WiFi.begin("LIUYU", "12345678");
  while (!WiFi.isConnected())
  {
    delay(500);
    Serial.print('.');
  }
  Serial.println(WiFi.localIP());
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, LOW);
  server.begin();

}

void move(int index, int from, int to)
{
  if (from == to) return;

  if (from < to)
  {
    for (int i = from; i <= to; i++)
    {
      servo[index].write(i);
      delay(10);
    }
  }

  if (from > to)
  {
    for (int i = from; i >= to; i--)
    {
      servo[index].write(i);
      delay(10);
    }
  }
}

void loop() {
  WiFiClient client = server.available();
  if (!client)
  {
    return;
  }

  Serial.print(client.remoteIP());
  Serial.println(" connected");

  while (client.connected())
  {
    if (client.available())
    {
      String input = client.readStringUntil('\n');
      input.trim();

      //{"servo":1,"angle":30}
      StaticJsonDocument<64> doc;
      DeserializationError error = deserializeJson(doc, input);

      if (error) {
        Serial.print(F("deserializeJson() failed: "));
        Serial.println(error.f_str());
        return;
      }

      int index = doc["servo"];
      int angle = doc["angle"];

      int orig = servo[index].read();

      Serial.println("Servo " + String(index) + " from " + String(orig) + " move to " + String(angle));
      move(index, orig, angle);
    }
  }
}
